#include "ros/ros.h"
#include "visualization_msgs/MarkerArray.h"
#include "visualization_msgs/Marker.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "bipedRobin_msgs/InitialStepsService.h"
#include "bipedRobin_msgs/SetModeService.h"
#include "bipedRobin_msgs/StepTarget3DService.h"
#include "std_srvs/Empty.h"
#include "std_msgs/UInt8.h"
#include <string.h>
 /*
 		Dieser Knoten übernimmt die Koordinierung zwischen Roboter und FootstepPlanner
		Services die Angeboten werden:
		Statische Planung
		Dynamsiche Planung
		Replan: Neuplanung der Schritte
		CreateMap: Neu Karte erstellen
		Localize: Lokalisieren des Roboters im Raum
		Walk: Durchführen der geplanten Schritte
 */
#define DEFAULT_VARIABLE_NUMBER 150
#define DEFAULT_WORDLENGTH 20   /* multible of 4 and compatible with simulink receiving file */

//Funktionen
void staticInitialSteps();
void setInitialSteps(tf::StampedTransform left, tf::StampedTransform right);
void setInitialStep();
void staticPlanning();
void dynamicPlanning();
void localize();
void createMap();
void goalpose();
void replan();
void restart();
void pub_goal();
void sendMarker(visualization_msgs::Marker step, tf::StampedTransform map);
void setdynamicInitialSteps(visualization_msgs::Marker left, visualization_msgs::Marker right, tf::StampedTransform map);

//Clients Subscriber Publisher
ros::ServiceClient initialStep_client;
ros::ServiceClient initialSteps_client;
ros::ServiceClient localize_client;
ros::ServiceClient createMap_client;
ros::ServiceClient footstep_client;
ros::ServiceClient goalpose_client;
ros::ServiceClient restart_client;
ros::Subscriber markerArray;
ros::Subscriber goalpose_sub;
ros::Publisher goal_pub;

//Flags
bool isReal;
bool sendStep_flag = false;
bool first_call_flag = true;
bool newMarkerArray_flag = false;
bool walk_flag = false;
bool plan_flag = false;
bool staticPlanner_flag = false;
bool finished = false;
bool random_flag = true;

int dynamic_left_right = 1;
int stepsLeftInBuffer = 0;
int dynamicBufferSize = 0;
int stepBufferSize = 0;

visualization_msgs::MarkerArray currentFootsteps;
geometry_msgs::PoseStamped currentGoalpose;
tf::StampedTransform old_left;
tf::StampedTransform old_right;

static tf::TransformListener* pListener = NULL;
static tf::TransformBroadcaster* pBroadcaster = NULL;

void localize(){ //Ruft die Lokalisierung auf
	ROS_INFO("localize Service called");
	std_srvs::Empty srv;
	localize_client.call(srv);
}

void createMap(){ //Erstellt eine neue Karte
	ROS_INFO("createMap Service called");
	std_srvs::Empty srv;	
	createMap_client.call(srv);		
}

void goalpose(){ //Published die Koordinaten des Zielmarkers falls vorhanden
	ROS_INFO("Automatic Goalpose Service called");
	std_srvs::Empty srv;
	goalpose_client.call(srv);
}

void restart(){ //Startet den FootstepPlanner neue
	ROS_INFO("Planner restart Service called");
	std_srvs::Empty srv;
	restart_client.call(srv);
}

void replan(){
	ROS_INFO("replan Service called");
	newMarkerArray_flag = false;	//neue Schritte zu lassen
	setInitialSteps(old_left, old_right); //alte Startschritte setzen	
	ros::spinOnce();	
	goalpose();  //einlesen der Zielmarker Position
	ros::spinOnce();			
	createMap(); //Neu erstellen der Karte
	ros::spinOnce();
	pub_goal(); //Ziel erneut an den FootstepPlanner schicken um neuPlanung zu starten.
}

void pub_goal(){ //Published das Ziel um eine neuplanung zu erwirken
	ROS_INFO("Goal republished");
	geometry_msgs::PoseStamped goalpose = currentGoalpose;	
	if(random_flag){ //Selbes Ziel führt zu keiner Neuplanung deswegen zufalls zahlen hinzu addieren
		goalpose.pose.position.x += (rand()%5) * 0.01;
		goalpose.pose.position.y += (rand()%5) * 0.01;
	}
	goalpose.header.stamp = ros::Time::now();
	goal_pub.publish(goalpose);	
}

//Sender einen als Schritt Koordinaten an den Roboter
void sendMarker(visualization_msgs::Marker step, tf::StampedTransform map){	
	tf::StampedTransform marker;
	tf::Vector3 marker_Vector;
	tf::Quaternion marker_Rotation;	
	marker_Vector.setX(step.pose.position.x);
	marker_Vector.setY(step.pose.position.y);
	marker_Vector.setZ(step.pose.position.z);
	marker_Rotation.setX(step.pose.orientation.x);
	marker_Rotation.setY(step.pose.orientation.y);
	marker_Rotation.setZ(step.pose.orientation.z);
	marker_Rotation.setW(step.pose.orientation.w);
	marker.setOrigin(marker_Vector);
	marker.setRotation(marker_Rotation);		
	marker.mult(map, marker);
	bipedRobin_msgs::StepTarget3DService srv;
	//unterscheiden zwischen linken und rechten fuß
	if(step.color.r > 0) { //links
		srv.request.step.leg = 1;
	} else { //rechts
		srv.request.step.leg = 0;
	}
	srv.request.step.pose.position.x = marker.getOrigin().x();
	srv.request.step.pose.position.y = marker.getOrigin().y();
	srv.request.step.pose.position.z = 0;
	srv.request.step.pose.orientation.x = marker.getRotation().x();
	srv.request.step.pose.orientation.y = marker.getRotation().y();
	srv.request.step.pose.orientation.z = marker.getRotation().z();
	srv.request.step.pose.orientation.w = marker.getRotation().w();									
	footstep_client.call(srv);
}

//Funktion für die eigentliche Schrittplanung
void dynamicPlanning(){
	restart();	//neustarten des Planners
	ros::Duration(3).sleep();
	ros::spinOnce();
	if(first_call_flag) {	//nur beim aller ersten Aufruf durchführen
		setInitialStep(); 
		localize();	
		ros::Duration(2).sleep();				
	} else {
		sendStep_flag = true;
	}		
	createMap();
	staticInitialSteps();	//Wirkliche Fuß Posiotenen als StartSchritte setzen.
	int currentStep = 0;  //Aktueller Schritt der gesendet werden soll
	tf::StampedTransform map;  //Transformation zu map Koordinaten
	visualization_msgs::Marker temp[stepBufferSize];
	visualization_msgs::Marker Buffer[stepBufferSize];  //dynamischer Buffer
	visualization_msgs::Marker left;
	visualization_msgs::Marker right;
	int stepsInDynamicBuffer = 0; //Schritte die noch im dynamischen Buffer liegen
	finished = false;  //Variable ob Zielposition erreicht 
	bool replan_flag = true;
	first_call_flag = true;
	walk_flag = false;
	ros::Rate loop_rate(100);
	try { 			   	
		pListener->lookupTransform("/odom", "/map", ros::Time(0), map);
	} catch (tf::TransformException ex) {							
	}
	while(ros::ok){
		if(replan_flag){				
			ROS_INFO("Planning started");			
			goalpose();			
			createMap();
			ros::spinOnce();
			pub_goal();
			newMarkerArray_flag = false;
			ROS_INFO("Waiting for walk approval");
			if(walk_flag){
				ROS_INFO("Waiting for new Footsteps from Planner");		
				while(!newMarkerArray_flag) ros::spinOnce(); loop_rate.sleep();	
				newMarkerArray_flag = false;
				int count = 0;
				while(!newMarkerArray_flag && (count < 30)){
					ros::spinOnce(); 
					loop_rate.sleep();	
					count++; 
				}
     		ROS_INFO("Footsteps received. executing");	
			}
			while(!walk_flag) {			
				ros::spinOnce(); 
				loop_rate.sleep();	
			}				
			first_call_flag = false;		
			replan_flag = false;			
			
			visualization_msgs::MarkerArray footsteps = currentFootsteps;
			int startStep = 2;

			if(fabs(footsteps.markers[2].pose.position.x - footsteps.markers[0].pose.position.x) < 0.05 && fabs(footsteps.markers[2].pose.position.y - footsteps.markers[0].pose.position.y) < 0.05){
				if(footsteps.markers.size() > 3){				
					startStep = 3;	
				}		
			}	
			if(((footsteps.markers[startStep].color.r > 0) && (dynamic_left_right == 1)) || ((footsteps.markers[startStep].color.g > 0) && (dynamic_left_right == 0))){
				startStep--;
			}				
			int i = startStep;
			int k = 0;			
			while(i < startStep+stepBufferSize && i < footsteps.markers.size()){
				temp[k] = footsteps.markers[i];				
				i++; k++; stepsInDynamicBuffer++;		
			}
			if(i == footsteps.markers.size()){
				finished = true;	
				if(temp[k-1].color.r > 0) {
					left = temp[k-1];
					dynamic_left_right = 1;	
				} else {
					right = temp[k-1];
					dynamic_left_right = 0;
				}
			}	else {
				finished = false;			
				if(temp[k-1].color.r > 0) {
					left = temp[k-1];
					right = temp[k-2];
					dynamic_left_right = 1;
				} else {
					right = temp[k-1];
					left = temp[k-2];
					dynamic_left_right = 0;
				}
			}
							
		}			
		
		if(currentStep == 0){
			for(int p = 0; p < stepBufferSize; p++){
				Buffer[p] = temp[p];				
			}			
			setdynamicInitialSteps(left, right, map);		
		}	
		
		if(currentStep == stepBufferSize) {
			currentStep = 0;
			replan_flag = true;
		}	

		if(finished && stepsInDynamicBuffer == 0){
			ROS_INFO("Reached Target destination");
			walk_flag = false;
			break;
		}
		
		if(sendStep_flag && !replan_flag){	
			sendStep_flag = false;
			sendMarker(Buffer[currentStep], map);
			currentStep++;
			stepsInDynamicBuffer--;
		}
		loop_rate.sleep();	
		ros::spinOnce();				
	}
	ros::spinOnce();
}

//Funktion zum setzen der Startschritte des FootstepPlanners auf bestimmte Positionen
void setdynamicInitialSteps(visualization_msgs::Marker leftMarker, visualization_msgs::Marker rightMarker, 	tf::StampedTransform map){
	ROS_INFO("setdynamicInitialSteps Service called");
	tf::StampedTransform left;
	tf::StampedTransform right;
	tf::Vector3 marker_Vector;
	tf::Quaternion marker_Rotation;	
	marker_Vector.setX(leftMarker.pose.position.x);
	marker_Vector.setY(leftMarker.pose.position.y);
	marker_Vector.setZ(leftMarker.pose.position.z);
	marker_Rotation.setX(leftMarker.pose.orientation.x);
	marker_Rotation.setY(leftMarker.pose.orientation.y);
	marker_Rotation.setZ(leftMarker.pose.orientation.z);
	marker_Rotation.setW(leftMarker.pose.orientation.w);
	left.setOrigin(marker_Vector);
	left.setRotation(marker_Rotation);
	bipedRobin_msgs::InitialStepsService srv;

	marker_Vector.setX(rightMarker.pose.position.x);
	marker_Vector.setY(rightMarker.pose.position.y);
	marker_Vector.setZ(rightMarker.pose.position.z);
	marker_Rotation.setX(rightMarker.pose.orientation.x);
	marker_Rotation.setY(rightMarker.pose.orientation.y);
	marker_Rotation.setZ(rightMarker.pose.orientation.z);
	marker_Rotation.setW(rightMarker.pose.orientation.w);
	right.setOrigin(marker_Vector);
	right.setRotation(marker_Rotation);	

	setInitialSteps(left, right);	
}

//Funktion zum Setzen der Startschritte des FootstepPlanners auf die aktuellen Positionen
void staticInitialSteps(){
	ROS_INFO("set staticInitialSteps Service called");
	tf::StampedTransform left;
	tf::StampedTransform right;

	try {
		pListener->lookupTransform("/map", "l_sole", ros::Time(0), left);
	} catch (tf::TransformException ex) {
	}

	try {
		pListener->lookupTransform("/map", "r_sole", ros::Time(0), right);
	} catch (tf::TransformException ex) {
	}
	setInitialSteps(left, right);	
}


//Funktion zum Senden der Startschritte des FootstepPlanners and den Planner
void setInitialSteps(tf::StampedTransform left, tf::StampedTransform right){
	
	old_left = left;  
	old_right = right;

	bipedRobin_msgs::InitialStepsService srv;
	
	srv.request.left.leg=1;	
	srv.request.left.pose.position.x = left.getOrigin().x();
	srv.request.left.pose.position.y = left.getOrigin().y();
	srv.request.left.pose.position.z = left.getOrigin().z();
	srv.request.left.pose.orientation.x = left.getRotation().x();
	srv.request.left.pose.orientation.y = left.getRotation().y();
	srv.request.left.pose.orientation.z = left.getRotation().z();
	srv.request.left.pose.orientation.w = left.getRotation().w();
	
	srv.request.right.leg=0;
	srv.request.right.pose.position.x = right.getOrigin().x();
	srv.request.right.pose.position.y = right.getOrigin().y();
	srv.request.right.pose.position.z = right.getOrigin().z();
	srv.request.right.pose.orientation.x = right.getRotation().x();
	srv.request.right.pose.orientation.y = right.getRotation().y();
	srv.request.right.pose.orientation.z = right.getRotation().z();
	srv.request.right.pose.orientation.w = right.getRotation().w();
	
	initialSteps_client.call(srv); 
}

//Funktion zum Setzen des InitialStep
void setInitialStep(){
	ROS_INFO("setInitialStep Service called");	
	tf::StampedTransform step;
	try {
		pListener->lookupTransform("/odom", "/l_sole", ros::Time(0), step);
	} catch (tf::TransformException ex) {
	}	
	bipedRobin_msgs::StepTarget3DService srv;	
	srv.request.step.leg=1;	
	srv.request.step.pose.position.x = step.getOrigin().x();
	srv.request.step.pose.position.y = step.getOrigin().y();
	srv.request.step.pose.position.z = step.getOrigin().z();
	srv.request.step.pose.orientation.x = step.getRotation().x();
	srv.request.step.pose.orientation.y = step.getRotation().y();
	srv.request.step.pose.orientation.z = step.getRotation().z();
	srv.request.step.pose.orientation.w = step.getRotation().w();
	initialStep_client.call(srv);
}

//Callback um statischen Planner zu starten
bool staticPlanningCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
	if(!plan_flag){	//Verhindern das Planner gestartet wird wenn ein andere läuft	
		ROS_INFO("staticPlanning Service started");
		stepBufferSize = 100; //Anzahl der Schritt bevor eine Neuplanung statt findet -> statisch
		staticPlanner_flag = true;	
		plan_flag = true; //flag um planner aufzurufen
	}
	return true;
}

//Callback um dynamischen Planner zu starten
bool dynamicPlanningCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
	if(!plan_flag){		
		ROS_INFO("dynamicPlanning Service started");
		stepBufferSize = dynamicBufferSize; //Anzahl der Schritte bevor Neuplanung statt findet default 3		
		staticPlanner_flag = false;
		plan_flag = true; //flag um planner aufzurufen
	}
	return true;
}

//Callback um freigabe zum Gehen zu erteilen
bool walkCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res){
	walk_flag = true;
	return true;
}

//Calback für neue Schritt aus FootstepPlanner
void newStepsArrayCallback(const visualization_msgs::MarkerArray markerArray) {
	bool ignore = false;
	//Verhindern das leere Schritte akzeptiert werden:
	for(int i=0; i < markerArray.markers.size(); i++){
		if(markerArray.markers[i].color.r == 0 && markerArray.markers[i].color.g == 0) ignore = true;	
	}
	//Nur wenn mindestens 3 Schritte und neue Schritte gefordert sind bzw. der staticPlanner läuft die Schritte abspeichern
	if(markerArray.markers.size() > 2 && !ignore && (!newMarkerArray_flag || staticPlanner_flag)){
		ROS_INFO("New Footsteps received");
		//Wenn noch mehr als 6 Schritte zu gehen sind (2 sind Startposition) dann das Ziel random Posten.		
		if(markerArray.markers.size() < 7){
			random_flag = false;
		} else {
			random_flag = true;
		}
		currentFootsteps = markerArray;
		newMarkerArray_flag = true;
	}	
}

//Callback für Aktuellen Schrittpuffer am Roboter
void stepsLeftCallback(std_msgs::UInt8 stepsLeft) {
	stepsLeftInBuffer = stepsLeft.data;	//Schritte für dynamischen Planner setzen
	if(stepsLeft.data == 0){ //keine Schritte mehr im Buffer
		ros::Duration(0.8).sleep(); //warten bevor neuer Schritt gesendet wird
		sendStep_flag = true; //neuen Schritt senden
	}
}

//Callback für aktuelles Ziel
void goalCallback(geometry_msgs::PoseStamped pose){
	newMarkerArray_flag = false;
	currentGoalpose = pose; //speichert das Ziel ab um es bei Bedarf neu im Planner zu setzen
	//replan();
}

//Callback um neuplanen der Schritte zu erzwingen
bool replanCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) 	{
	replan();
	return true;
}

int main(int argc, char **argv)
{
  /* init ROS */  
  
	ros::init(argc, argv, "navigation_master");
	pListener = new(tf::TransformListener);
	pBroadcaster = new(tf::TransformBroadcaster);	

	ros::NodeHandle n;
	
	n.param("/par_isRealEnvironment", isReal, false); //real oder simulated
	n.param("/par_stepBufferSize", dynamicBufferSize, 5); //Anzahl der Schritt bevor Neuplanung stattfindet

	//Service Server
	ros::ServiceServer staticPlanning_srv = n.advertiseService("staticPlanning_srv", staticPlanningCallback);
	ros::ServiceServer dynamicPlanning_srv = n.advertiseService("dynamicPlanning_srv", dynamicPlanningCallback);
	ros::ServiceServer walk_srv = n.advertiseService("walk_srv", walkCallback);
	ros::ServiceServer replan_srv = n.advertiseService("replan_srv", replanCallback);
	
	//subscriber
	markerArray = n.subscribe<visualization_msgs::MarkerArray>("bipedRobin_footstep_planner/footsteps_array", 1, newStepsArrayCallback);
	ros::Subscriber stepsLeftInBuffer_sub = n.subscribe<std_msgs::UInt8>("stepsLeftInBuffer", 1, stepsLeftCallback);
	goalpose_sub = n.subscribe<geometry_msgs::PoseStamped>("goal2", 0, goalCallback);

	//Service clients
	initialStep_client = n.serviceClient<bipedRobin_msgs::StepTarget3DService>("initialstep_srv");  
	initialSteps_client = n.serviceClient<bipedRobin_msgs::InitialStepsService>("initialSteps_srv");		
	localize_client = n.serviceClient<std_srvs::Empty>("localize_srv");			
	restart_client = n.serviceClient<std_srvs::Empty>("restart_srv");
	createMap_client = n.serviceClient<std_srvs::Empty>("createMap_srv");
	footstep_client = n.serviceClient<bipedRobin_msgs::StepTarget3DService>("footstep3D_srv");	
	goalpose_client = n.serviceClient<std_srvs::Empty>("goalpose_srv");
	
	//publisher
	goal_pub = n.advertise<geometry_msgs::PoseStamped>("goal", 1000);
	

	//Schleife um Planung zu starten, notwendig damit Service return sendet ohne auf Fertigstellung des Planners zu warten.
	while(ros::ok()){
		if(plan_flag){
			dynamicPlanning();
		}
		plan_flag = false;
		ros::spinOnce();
	}

	return 0; 
}


